#include "serial.h"
#include <opencv2/opencv.hpp>
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include <cstring>
#include <iostream>
#include <sys/select.h>
#include <sys/time.h>
#include <errno.h>
#include <stdexcept>

Serial::Serial() : virtual_serial_(false), fd_(-1) {
}

Serial::~Serial() {
    close();
}

bool Serial::init(const std::string& config_path) {
    try {
        cv::FileStorage fs(config_path, cv::FileStorage::READ);
        if (!fs.isOpened()) {
            std::cerr << "Failed to open config file: " << config_path << std::endl;
            return false;
        }

        fs["port"] >> port_;
        fs["baudrate"] >> baudrate_;
        fs["flow_control"] >> flow_control_;
        fs["parity"] >> parity_;
        fs["stop_bits"] >> stop_bits_;
        fs["virtual_serial"] >> virtual_serial_;

        if (virtual_serial_) {
            // 虚拟串口模式 - 读取虚拟数据配置
            cv::FileNode virtual_data_node = fs["virtual_data"];
            virtual_data_.yaw = virtual_data_node["yaw"];
            virtual_data_.pitch = virtual_data_node["pitch"];
            virtual_data_.roll = virtual_data_node["roll"];
            virtual_data_.timestamp = std::chrono::steady_clock::now();
            fs.release();
            std::cout << "Virtual serial mode enabled" << std::endl;
            return true;
        }

        fs.release();

        // 实际串口模式 - 打开串口设备
        std::cout << "Opening serial port: " << port_ << std::endl;
        // 初始化时只尝试一次，失败则返回false
        return openPort();
    } catch (const std::exception& ex) {
        std::cerr << "Exception during serial initialization: " << ex.what() << std::endl;
        return false;
    }
}

void Serial::close() {
    try {
        if (!virtual_serial_ && fd_ != -1) {
            closePort();
        }
    } catch (const std::exception& ex) {
        std::cerr << "Exception during serial close: " << ex.what() << std::endl;
    }
}

bool Serial::readData(ReceivedData& data) {
    try {
        if (virtual_serial_) {
            // 虚拟串口模式 - 返回预设数据
            virtual_data_.timestamp = std::chrono::steady_clock::now();
            data = virtual_data_;
            usleep(10000); // 10ms delay to simulate real serial communication
            return true;
        }

        // 实际串口模式 - 从串口读取数据
        // 帧格式: 帧头(0xAA) + yaw(4字节) + pitch(4字节) + roll(4字节) + 帧尾(0x5A)
        std::vector<uint8_t> buffer(sizeof(ReceivePacket));
        if (!readFromPort(buffer, sizeof(ReceivePacket))) {
            // 读取失败，尝试重新打开端口
            if (!reopenPort()) {
                return false;
            }
            // 重连成功后再次尝试读取
            if (!readFromPort(buffer, sizeof(ReceivePacket))) {
                return false;
            }
        }

        // 检查帧头和帧尾
        const uint8_t expected_header = 0xAA;
        const uint8_t expected_tail = 0x5A;

        if (buffer[0] != expected_header || buffer[sizeof(ReceivePacket) - 1] != expected_tail) {
            std::cerr << "Invalid header or tail: " << std::hex << static_cast<int>(buffer[0])
                      << " " << static_cast<int>(buffer[sizeof(ReceivePacket) - 1]) << std::dec << std::endl;
            return false;
        }

        // 解析数据包
        ReceivePacket packet;
        std::memcpy(&packet, buffer.data(), sizeof(ReceivePacket));

        // 解析数据
        data.yaw = static_cast<double>(packet.yaw);
        data.pitch = static_cast<double>(packet.pitch);
        data.roll = static_cast<double>(packet.roll);
        data.timestamp = std::chrono::steady_clock::now();

        return true;
    } catch (const std::exception& ex) {
        std::cerr << "Exception during readData: " << ex.what() << std::endl;
        // 尝试重新打开端口
        if (!virtual_serial_) {
            reopenPort();
        }
        return false;
    }
}

bool Serial::sendData(const SendPacket& data) {
    try {
        if (virtual_serial_) {
            // 虚拟串口模式 - 不执行任何操作
            return true;
        }

        // 实际串口模式 - 发送数据到串口
        std::vector<uint8_t> data_vector(reinterpret_cast<const uint8_t*>(&data),
                                         reinterpret_cast<const uint8_t*>(&data) + sizeof(SendPacket));

        if (!writeToPort(data_vector)) {
            // 发送失败，尝试重新打开端口
            if (!reopenPort()) {
                return false;
            }
            // 重连成功后再次尝试发送
            if (!writeToPort(data_vector)) {
                return false;
            }
        }

        return true;
    } catch (const std::exception& ex) {
        std::cerr << "Exception during sendData: " << ex.what() << std::endl;
        // 尝试重新打开端口
        if (!virtual_serial_) {
            reopenPort();
        }
        return false;
    }
}

bool Serial::openPort() {
    try {
        fd_ = ::open(port_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
        if (fd_ == -1) {
            std::cerr << "Failed to open serial port: " << port_ << std::endl;
            return false;
        }

        // 设置为阻塞模式
        int flags = fcntl(fd_, F_GETFL, 0);
        fcntl(fd_, F_SETFL, flags & ~O_NONBLOCK);

        struct termios options;
        tcgetattr(fd_, &options);

        // 设置波特率
        switch (baudrate_) {
            case 9600:
                cfsetispeed(&options, B9600);
                cfsetospeed(&options, B9600);
                break;
            case 19200:
                cfsetispeed(&options, B19200);
                cfsetospeed(&options, B19200);
                break;
            case 38400:
                cfsetispeed(&options, B38400);
                cfsetospeed(&options, B38400);
                break;
            case 57600:
                cfsetispeed(&options, B57600);
                cfsetospeed(&options, B57600);
                break;
            case 115200:
                cfsetispeed(&options, B115200);
                cfsetospeed(&options, B115200);
                break;
            default:
                std::cerr << "Unsupported baudrate: " << baudrate_ << std::endl;
                closePort();
                return false;
        }

        // 设置数据位、停止位和校验位
        options.c_cflag |= (CLOCAL | CREAD);

        // 设置校验位
        if (parity_ == "none") {
            options.c_cflag &= ~PARENB;  // 无校验
        } else if (parity_ == "even") {
            options.c_cflag |= PARENB;   // 启用校验
            options.c_cflag &= ~PARODD;  // 偶校验
        } else if (parity_ == "odd") {
            options.c_cflag |= PARENB;   // 启用校验
            options.c_cflag |= PARODD;   // 奇校验
        }

        // 设置停止位
        if (stop_bits_ == "1") {
            options.c_cflag &= ~CSTOPB;  // 1个停止位
        } else if (stop_bits_ == "2") {
            options.c_cflag |= CSTOPB;   // 2个停止位
        }

        // 设置数据位
        options.c_cflag &= ~CSIZE;
        options.c_cflag |= CS8;          // 8个数据位

        // 设置流控制
        if (flow_control_ == "none") {
            options.c_cflag &= ~CRTSCTS;     // 关闭硬件流控
            options.c_iflag &= ~(IXON | IXOFF | IXANY);  // 关闭软件流控
        } else if (flow_control_ == "hardware") {
            options.c_cflag |= CRTSCTS;      // 启用硬件流控
            options.c_iflag &= ~(IXON | IXOFF | IXANY);  // 关闭软件流控
        } else if (flow_control_ == "software") {
            options.c_cflag &= ~CRTSCTS;     // 关闭硬件流控
            options.c_iflag |= (IXON | IXOFF); // 启用软件流控
        }

        // 设置最小字符数和超时
        options.c_cc[VMIN] = 0;   // 最小字符数
        options.c_cc[VTIME] = 5;  // 超时时间 (0.5秒)

        options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);  // 原始输入
        options.c_oflag &= ~OPOST;                           // 原始输出

        // 激活配置
        tcsetattr(fd_, TCSANOW, &options);

        return true;
    } catch (const std::exception& ex) {
        std::cerr << "Exception during openPort: " << ex.what() << std::endl;
        if (fd_ != -1) {
            ::close(fd_);
            fd_ = -1;
        }
        return false;
    }
}

void Serial::closePort() {
    try {
        if (fd_ != -1) {
            ::close(fd_);
            fd_ = -1;
        }
    } catch (const std::exception& ex) {
        std::cerr << "Exception during closePort: " << ex.what() << std::endl;
        fd_ = -1; // 强制设置为-1
    }
}

bool Serial::readFromPort(std::vector<uint8_t>& buffer, size_t size) {
    try {
        if (fd_ == -1) return false;

        buffer.resize(size);
        size_t bytes_read = 0;
        while (bytes_read < size) {
            ssize_t result = ::read(fd_, buffer.data() + bytes_read, size - bytes_read);
            if (result < 0) {
                if (errno == EAGAIN || errno == EWOULDBLOCK) {
                    // 非阻塞模式下的超时，继续尝试
                    continue;
                }
                std::cerr << "Error reading from serial port: " << strerror(errno) << std::endl;
                return false;
            } else if (result == 0) {
                // EOF 或连接断开
                std::cerr << "Serial port connection lost" << std::endl;
                return false;
            }
            bytes_read += result;
        }

        return true;
    } catch (const std::exception& ex) {
        std::cerr << "Exception during readFromPort: " << ex.what() << std::endl;
        return false;
    }
}

bool Serial::writeToPort(const std::vector<uint8_t>& data) {
    try {
        if (fd_ == -1) return false;

        size_t bytes_written = 0;
        while (bytes_written < data.size()) {
            ssize_t result = ::write(fd_, data.data() + bytes_written, data.size() - bytes_written);
            if (result < 0) {
                std::cerr << "Error writing to serial port: " << strerror(errno) << std::endl;
                return false;
            }
            bytes_written += result;
        }

        return true;
    } catch (const std::exception& ex) {
        std::cerr << "Exception during writeToPort: " << ex.what() << std::endl;
        return false;
    }
}

bool Serial::reopenPort() {
    try {
        std::cerr << "Attempting to reopen port: " << port_ << std::endl;

        // 先关闭端口
        if (fd_ != -1) {
            closePort();
        }

        // 持续重试直到成功
        int attempt = 0;
        while (true) {
            attempt++;
            std::this_thread::sleep_for(std::chrono::seconds(1));
            if (openPort()) {
                std::cout << "Successfully reopened port: " << port_ << " (attempt " << attempt << ")" << std::endl;
                return true;
            }
            std::cerr << "Failed to reopen port, attempt " << attempt << ", retrying..." << std::endl;
        }

    } catch (const std::exception& ex) {
        std::cerr << "Exception during reopenPort: " << ex.what() << std::endl;
        return false;
    }
}
